﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Sharp3D.Math.Core;

namespace GLWrap.MathExt
{
    public static class QuaternionFExtension
    {
        public static Matrix4F GetRotationMatrix(this QuaternionF q)
        {
            Matrix4F m = Matrix4F.Identity;
            float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
            float s = 2.0f / q.GetModulusSquared();  // 4 mul 3 add 1 div
            x2 = q.X * s; y2 = q.Y * s; z2 = q.Z * s;
            xx = q.X * x2; xy = q.X * y2; xz = q.X * z2;
            yy = q.Y * y2; yz = q.Y * z2; zz = q.Z * z2;
            wx = q.W * x2; wy = q.W * y2; wz = q.W * z2;

            m[1,1] = 1.0f - (yy + zz);
            m[2,1] = xy - wz;
            m[3,1] = xz + wy;

            m[1,2] = xy + wz;
            m[2,2] = 1.0f - (xx + zz);
            m[3,2] = yz - wx;
            
            m[1,3] = xz - wy;
            m[2,3] = yz + wx;
            m[3,3] = 1.0f - (xx + yy);

            return m;
        }

        public static QuaternionF CreateWithAxisAndAngle(Vector3F axis, float angleInDegress)
        {
            if(axis.GetLength()!=0.0f)
            axis.Normalize();
            float half_angle = angleInDegress.DegreesToRadians() * 0.5f;
            float sin_a = (float)Math.Sin(half_angle);
            return new QuaternionF((float)Math.Cos(half_angle), axis.X * sin_a, axis.Y * sin_a, axis.Z * sin_a);
        }

        public static void GetAxisAndAngle(this QuaternionF q, ref Vector3F a, ref float angle)
        {
            float vl = (float)Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);
            if (vl > 0.0001f)
            {
                float ivl = 1.0f / vl;
                a.X = q.X * ivl;
                a.Y = q.Y * ivl;
                a.Z = q.Z * ivl;
                
                if (q.W < 0)
                    angle = 2.0f * (float)Math.Atan2(-vl, -q.W); //-PI,0 
                else
                    angle = 2.0f * (float)Math.Atan2(vl, q.W); //0,PI 
            }
            else
            {
                a = new Vector3F(0, 0, 0);
                angle = 0;
            }
        }
    }
}
